// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GlobalMapModel.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Visualization
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Windows;
    using System.Windows.Media;     

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Visualization;

    /// <summary>
    /// Model for visualizing the global map
    /// </summary>
    public sealed class GlobalMapModel : Model
    {
        /// <summary>
        /// The max length of robot path pose history
        /// </summary>
        private const int RobotPathMax = 10000;

        /// <summary>
        /// The maximum number of poses we show
        /// </summary>
        private const int MaxRenderedPoses = 250;

        /// <summary>
        /// The warning level for dispersion increase
        /// </summary>
        private const double DispersionWarningLevel = 5;

        /// <summary>
        /// The error level for dispersion increase
        /// </summary>
        private const double DispersionErrorLevel = 10;

        /// <summary>
        /// The set of properties we might not want the view to refresh
        /// </summary>
        private readonly string[] unchangedProperties = new[] { "GlobalMapVideoFrame" };

        /// <summary>
        /// Save the starting location of a mouse drag
        /// </summary>
        private Point mouseStart;

        /// <summary>
        /// Save the current location of a mouse move
        /// </summary>
        private Point mouseMove;
        
        /// <summary>
        /// The image we generate from the global map.
        /// </summary>
        private byte[] rawImage;

        /// <summary>
        /// The startup pose of the map 
        /// </summary>
        private Pose2D mapStartPose;

        /// <summary>
        /// Do we want the UI to refresh the global map
        /// </summary>
        private bool mapChanged;

        /// <summary>
        /// The hashcode of the last map. No need to hold on to the reference to delay GC.
        /// </summary>
        private int lastMapHash;

        /// <summary>
        /// The timestamp of the last map. 
        /// </summary>
        private DateTime lastMapTimestamp;

        /// <summary>
        /// The maximum depth to use when rendering the HDPs 
        /// </summary>
        private double hdpDistanceCutoff;

        /// <summary>
        /// The particle we are currently displaying
        /// </summary>
        private int selectedParticleId;

        /// <summary>
        /// Initializes a new instance of the GlobalMapModel class.
        /// </summary>
        public GlobalMapModel()
        {
            this.RobotVisibility = Visibility.Hidden;
            this.RobotPath = new ConcurrentBag<Pose2D>();
        }

        /// <summary>
        /// Gets or sets the mouse move position for showing the current map coordinates
        /// </summary>
        public Point MouseMove
        {
            get
            {
                return this.mouseMove;
            }

            set
            {
                if (value.X >= 0 && value.Y >= 0)
                {
                    this.mouseMove = value;
                    this.MapCoordinates = string.Format(
                        " Pixels X {0:N0} Y {1:N0}         Meters X {2:N4}m Y {3:N4}m ",
                        this.mouseMove.Y,
                        this.mouseMove.X,
                        this.DistanceResolution * (this.mouseMove.Y),
                        this.DistanceResolution * (this.mouseMove.X));
                }
                else
                {
                    this.MapCoordinates = string.Empty;
                }
            }
        }

        /// <summary>
        /// Gets or sets the mouse start position
        /// </summary>
        public Point MouseStart
        {
            get
            {
                return this.mouseStart;
            }

            set
            {
                this.mouseStart = value;                
            }
        }

        /// <summary>
        /// Gets or sets the map coordinates text
        /// </summary>
        public string MapCoordinates
        {
            get;
            set;
        }

        /// <summary>
        /// Gets or sets the mouse end position
        /// </summary>
        public Point MouseEnd
        {
            get
            {
                return this.mouseStart;
            }

            set
            {
                // Switch X and Y since our maps are X in the up direction.
                double heading = Math.Atan2(value.X - this.mouseStart.X, value.Y - this.mouseStart.Y);
                this.ClickToLocalizeDelegate(new Pose2D(this.mouseStart.Y, this.mouseStart.X, heading));                
            }
        }

        /// <summary>
        /// Gets or sets the delegate that controls what is done on a mouse click.
        /// </summary>
        public Action<Pose2D> ClickToLocalizeDelegate { get; set; }

        /// <summary>
        /// Gets the resolution of the map being visualized, in meters
        /// </summary>
        public double DistanceResolution { get; private set; }

        /// <summary>
        /// Gets or sets the map height
        /// </summary>
        public double GlobalMapImageHeight { get; set; }

        /// <summary>
        /// Gets or sets the map width
        /// </summary>
        public double GlobalMapImageWidth { get; set; }

        /// <summary>
        /// Gets or sets the depth image.
        /// </summary>
        public VideoFrame GlobalMapVideoFrame { get; set; }

        /// <summary>
        /// Gets or sets the map width
        /// </summary>
        public double GlobalMapWidth { get; set; }

        /// <summary>
        /// Gets or sets the map height
        /// </summary>
        public double GlobalMapHeight { get; set; }

        /// <summary>
        /// Gets or sets the origin pose.
        /// </summary>
        public Pose2D Origin { get; set; }

        /// <summary>
        /// Gets the model class used to show the HDP, if one is provided
        /// </summary>
        public HorizontalDepthProfileModel HdpModel { get; private set; }

        /// <summary>
        /// Gets or sets the local map pose X.
        /// </summary>
        public double HdpX { get; set; }

        /// <summary>
        /// Gets or sets the local map pose Y.
        /// </summary>
        public double HdpY { get; set; }

        /// <summary>
        /// Gets or sets the local map pose Angle.
        /// </summary>
        public double HdpAngle { get; set; }

        /// <summary>
        /// Gets the model class used to show the re-projected HDP, if one is provided
        /// </summary>
        public HorizontalDepthProfileModel SurfaceModel { get; private set; }

        /// <summary>
        /// Gets or sets the local map pose X.
        /// </summary>
        public double SurfaceX { get; set; }

        /// <summary>
        /// Gets or sets the local map pose Y.
        /// </summary>
        public double SurfaceY { get; set; }

        /// <summary>
        /// Gets or sets the local map pose Angle.
        /// </summary>
        public double SurfaceAngle { get; set; }

        /// <summary>
        /// Gets or sets the minimum distance in the recovered surface
        /// </summary>
        public double MinDistanceToSurface { get; set; }

        /// <summary>
        /// Gets or sets the maximum distance in the recovered surface
        /// </summary>
        public double MaxDistanceToSurface { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether the HDP should be displayed
        /// </summary>
        public bool IsHdpHidden { get; set; }

        /// <summary>
        /// Gets or sets the particles poses.
        /// </summary>
        public IEnumerable<Pose2D> ParticlePoses { get; set; }

        /// <summary>
        /// Gets or sets the particles received from the agent. Could be null.
        /// </summary>
        public SLAM2DParticle[] ParticleDetails { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether map cells should be displayed according to their weights
        /// </summary>
        public bool ShowAllReadings { get; set; }

        /// <summary>
        /// Gets or sets a measure of spatial variance of the particle set
        /// </summary>
        public double Dispersion { get; set; }

        /// <summary>
        /// Gets or sets the color of the robot, blue under normal circumstances and yellow/red when dispersion is too high.
        /// </summary>
        public Brush RobotColor { get; set; }

        /// <summary>
        /// Gets or sets particle selected for display (+1 since the UI slider is in the 1 -count rage
        /// </summary>
        public int SelectedParticleId 
        {
            get 
            { 
                return this.selectedParticleId + 1; 
            }

            set 
            { 
                this.selectedParticleId = value - 1;
                this.UpdateCurrentParticle();
                this.NotifyPropertyChanged();
            }
        }

        /// <summary>
        /// Gets the robot path (note that this is map path and not WPF path) data.
        /// </summary>
        public ConcurrentBag<Pose2D> RobotPath
        {
            get;
            private set;
        }

        /// <summary>
        /// Gets or sets a value indicating the robot center.
        /// </summary>
        public Point RobotCenter { get; set; }

        /// <summary>
        /// Gets a value indicating the robot center for the Ellipse.
        /// </summary>
        public Point RobotCenterForEllipse
        {
            get
            {
                return new Point(this.RobotCenter.X - this.RadiusX, this.RobotCenter.Y - this.RadiusY);
            }
        }

        /// <summary>
        /// Gets the weight of the selected particle
        /// </summary>
        public double SelectedParticleWeight
        {
            get
            {
                if (this.ParticleDetails == null)
                {
                    return 0;
                }

                return this.ParticleDetails[this.selectedParticleId].Weight;
            }
        }

        /// <summary>
        /// Gets the fitness to the last observation of the selected particle
        /// </summary>
        public double SelectedParticleSingleIterationWeight
        {
            get
            {
                if (this.ParticleDetails == null)
                {
                    return 0;
                }

                return this.ParticleDetails[this.selectedParticleId].SingleIterationWeight;
            }
        }

        /// <summary>
        /// Gets the update count of the selected particle
        /// </summary>
        public int SelectedParticleAge
        {
            get
            {
                if (this.ParticleDetails == null)
                {
                    return 0;
                }

                return this.ParticleDetails[this.selectedParticleId].UpdateCount;
            }
        }

        /// <summary>
        /// Gets the pose of the selected particle
        /// </summary>
        public Pose2D SelectedParticlePose
        {
            get
            {
                if (this.ParticleDetails == null)
                {
                    return new Pose2D();
                }

                return this.ParticleDetails[this.selectedParticleId].Pose;
            }
        }

        /// <summary>
        /// Gets a value indicating whether the model has particle details. This shows/hides the particle selector bar.
        /// </summary>
        public Visibility HasParticleDetails
        {
            get
            {
                return (this.ParticleDetails != null) ? Visibility.Visible : Visibility.Hidden;
            }
        }

        /// <summary>
        /// Gets a value indicating whether the model has particle details. This shows/hides the particle selector bar.
        /// </summary>
        public Visibility HdpVisibility
        {
            get
            {
                return (this.IsHdpHidden) ? Visibility.Hidden : Visibility.Visible;
            }
        }

        /// <summary>
        /// Gets the radius x of the robot ellipse
        /// </summary>
        public int RadiusX
        {
            get
            {
                return 3;
            }
        }

        /// <summary>
        /// Gets the radius y of the robot ellipse
        /// </summary>
        public int RadiusY
        {
            get
            {
                return 3;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating the robot end point.
        /// </summary>
        public Point RobotEnd { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether the robot is visible.
        /// </summary>
        public Visibility RobotVisibility { get; set; }

        /// <summary>
        /// The set of properties that have not changed. If null, all properties are updated. Override to filter which properties to skip.
        /// </summary>
        /// <returns>The properties to *not* refresh</returns>
        public override IEnumerable<string> GetUnchangedProperties()
        {
            if (!this.mapChanged)
            {
                return this.unchangedProperties;
            }

            return null;
        }

        /// <summary>
        /// Initializes the visualization with a map loaded from file
        /// </summary>
        /// <param name="mapFile">The map file to load</param>
        public void InitializeWithMap(string mapFile)
        {
            if (mapFile != null && File.Exists(mapFile))
            {
                GlobalMap map = Serializer.Load<GlobalMap>(mapFile);
                this.UpdateMap(map);
            }
        }

        /// <summary>
        /// Updates the map visualization
        /// </summary>
        /// <param name="map">The map to show. </param>
        public void UpdateFromMap(GlobalMap map)
        {
            this.mapChanged = true; //// (this.lastMapHash != map.GetHashCode() || this.lastMapTimestamp != map.LastUpdateTime);
            if (!this.mapChanged)
            {
                return;
            }

            this.lastMapHash = map.GetHashCode();
            this.lastMapTimestamp = map.LastUpdateTime;

            if (this.GlobalMapHeight != map.Height || this.GlobalMapWidth != map.Width)
            {
                this.GlobalMapWidth = map.Width;
                this.GlobalMapHeight = map.Height;
                this.DistanceResolution = map.DistanceResolution;
                this.mapStartPose = map.MapStartPose;
            }

            this.UpdateMap(map);
        }

        /// <summary>
        /// Updates the pose visualization
        /// </summary>
        /// <param name="mostLikelyPose">The main robot pose</param>
        /// <param name="poses">The other possible poses</param>
        public void UpdateFromPoses(Pose2D mostLikelyPose, IEnumerable<Pose2D> poses)
        {
            this.UpdatePoses(poses);
            this.UpdateRobotPose(mostLikelyPose);
            this.UpdateOrigin();
        }

        /// <summary>
        /// Update the image with the hdp information.
        /// </summary>            
        /// <param name="hdp">The horizontal depth profile to visualize</param>
        /// <param name="pose">The pose of the robot in map coordinates</param>
        /// <param name="hdpDistanceCutoff">The cutoff for the hdp visualization</param>
        public void UpdateFromHdp(IHorizontalDepthProfile hdp, Pose2D pose, double hdpDistanceCutoff)
        {
            if (this.HdpModel == null)
            {
                this.HdpModel = new HorizontalDepthProfileModel();
            }

            Pose2D cameraPose = GlobalMap.GetCameraPoseInMapCoordinates(pose, new Pose2D(hdp.Pose.X / this.DistanceResolution, hdp.Pose.Y / this.DistanceResolution, hdp.Pose.Heading));
            Color translucentYellow = Color.FromArgb(0x80, 0xFF, 0xFF, 0);
            this.HdpModel.UpdateWithHdp(hdp, hdpDistanceCutoff, this.DistanceResolution, Colors.OrangeRed, translucentYellow, Colors.Transparent);
            this.HdpX = cameraPose.Y - this.HdpModel.VideoFrame.Size.Width / 2d;
            this.HdpY = cameraPose.X;
            this.HdpAngle = -cameraPose.Heading * MathConstants.Radians2Degrees;
            this.hdpDistanceCutoff = hdpDistanceCutoff;
        }

        /// <summary>
        /// Updates the path we trace given the most likely pose
        /// </summary>
        /// <param name="mostLikelyPose">The pose to trace</param>
        public void UpdatePath(Pose2D mostLikelyPose)
        {
            this.RobotPath.Add(mostLikelyPose);
        }

        /// <summary>
        /// Updates the UI with particle internal data and aligns the hdp and re-projected surface with the particle orientation
        /// </summary>
        /// <param name="particles">The set of particles to update with</param>
        /// <param name="hdp">The horizontal depth profile to visualize</param>
        /// <param name="hdpDistanceCutoff">The cutoff for the surface and hdp visualization</param>
        public void UpdateFromParticles(SLAM2DParticle[] particles, IHorizontalDepthProfile hdp, double hdpDistanceCutoff)            
        {
            this.ParticleDetails = particles.OrderBy(p => -p.Weight).ToArray();
            this.ParticlePoses = this.ParticleDetails.Take(MaxRenderedPoses).Select(p => new Pose2D(p.Pose.X, p.Pose.Y, p.Pose.Heading));
            this.hdpDistanceCutoff = hdpDistanceCutoff;
            this.UpdateCurrentParticle();
            this.UpdateFromHdp(hdp, this.ParticleDetails[this.selectedParticleId].Pose, hdpDistanceCutoff);
        }

        /// <summary>
        /// Converts pose to map pose
        /// </summary>
        /// <param name="pose">Pose to be converted</param>
        /// <returns>Converted pose</returns>
        public Pose2D GetMapPoseInGlobalFrame(Pose2D pose)
        {
            Pose2D mapPose = new Pose2D(
                Math.Round(pose.X / this.DistanceResolution + this.mapStartPose.X),
                Math.Round(pose.Y / this.DistanceResolution + this.mapStartPose.Y),
                pose.Heading + this.mapStartPose.Heading);

            return mapPose;
        }

        /// <summary>
        /// Visualize Robot pose
        /// </summary>
        /// <param name="pose">The new pose of the robot</param>
        public void UpdateRobotPose(Pose2D pose)
        {
            // compute the dispersion
            double minX = this.ParticlePoses.Select(p => p.X).Min();
            double minY = this.ParticlePoses.Select(p => p.Y).Min();
            double minH = this.ParticlePoses.Select(p => p.Heading).Min();
            double maxX = this.ParticlePoses.Select(p => p.X).Max();
            double maxY = this.ParticlePoses.Select(p => p.Y).Max();
            double maxH = this.ParticlePoses.Select(p => p.Heading).Max();

            this.Dispersion = maxX - minX + maxY - minY + 2 * Pose2D.ClampAngle(maxH - minH);
            if (this.Dispersion > DispersionErrorLevel)
            {
                this.RobotColor = Brushes.Red;
            }
            else if (this.Dispersion > DispersionWarningLevel)
            {
                this.RobotColor = Brushes.DarkRed;
            }
            else
            {
                this.RobotColor = Brushes.Navy;
            }

            // update the robot position
            const int RobotOrientationLength = 20;
            int robotEndPointX = (int)Math.Round(pose.X + RobotOrientationLength * Math.Cos(pose.Heading));
            int robotEndPointY = (int)Math.Round(pose.Y + RobotOrientationLength * Math.Sin(pose.Heading));
            this.RobotCenter = new System.Windows.Point(pose.Y, pose.X);
            this.RobotEnd = new System.Windows.Point(robotEndPointY, robotEndPointX);
        }

        /// <summary>
        /// Update the image with the particle pose and re-projected surface information.
        /// </summary>            
        private void UpdateCurrentParticle()
        {
            IHorizontalDepthProfile surface = this.ParticleDetails[this.selectedParticleId].SurfaceFromMap;
            Pose2D particlePose = this.ParticleDetails[this.selectedParticleId].Pose;
            
            this.UpdateFromMap(this.ParticleDetails[this.selectedParticleId].Map);
            if (surface != null)
            {
                if (this.SurfaceModel == null)
                {
                    this.SurfaceModel = new HorizontalDepthProfileModel();
                }

                this.SurfaceModel.UpdateWithHdp(surface, this.hdpDistanceCutoff, this.DistanceResolution, Colors.Orange, Colors.Transparent, Colors.Transparent);
                Pose2D cameraPose = GlobalMap.GetCameraPoseInMapCoordinates(particlePose, new Pose2D(surface.Pose.X / this.DistanceResolution, surface.Pose.Y / this.DistanceResolution, surface.Pose.Heading));
                this.HdpX = this.SurfaceX = cameraPose.Y - this.SurfaceModel.VideoFrame.Size.Width / 2;
                this.HdpY = this.SurfaceY = cameraPose.X;
                this.HdpAngle = this.SurfaceAngle = -cameraPose.Heading * MathConstants.Radians2Degrees;
                this.MinDistanceToSurface = double.MaxValue;
                this.MaxDistanceToSurface = 0;
                for (int i = 0; i < surface.Length; i++)
                {
                    double depth = surface.GetHitPoint(i).X;
                    if (this.MinDistanceToSurface > depth && depth != 0)
                    {
                        this.MinDistanceToSurface = depth;
                    }

                    if (this.MaxDistanceToSurface < depth)
                    {
                        this.MaxDistanceToSurface = depth;
                    }
                }
            }
        }

        /// <summary>
        /// Visualize slam particles.
        /// </summary>
        /// <param name="poses">The set of poses to render</param>
        private void UpdatePoses(IEnumerable<Pose2D> poses)
        {
            if (poses != null)
            {
                // update the poses, up to a max value beyond which the rendering system cannot keep up
                // Note: this is because we databind a Path object (which is done instead of manipulating pixels in the map bitmap, 
                // because we don't want to repaint the map on every iteration if the map has not changed).
                this.ParticlePoses = poses.Take(MaxRenderedPoses);
            }
        }

        /// <summary>
        /// Visualize the map
        /// </summary>
        /// <param name="map">The map to update with</param>
        private void UpdateMap(GlobalMap map)
        {
            const byte FarBehind = 0;
            const byte NearBehind = 128;
            const byte FarFront = 128;
            const byte NearFront = 128;
            const byte Neutral = 127;
            byte farFront = this.ShowAllReadings ? FarBehind : FarFront;
            int width = map.Width;
            int height = map.Height;
            int bytesPerPixel = PixelFormats.Gray8.BitsPerPixel / 8;
            int stride = width * bytesPerPixel;
            if (this.rawImage == null)
            {
                this.rawImage = new byte[height * stride];
            }

            int index = 0;
            for (int row = 0; row < height; row++)
            {
                for (int col = 0; col < width; col++, index += bytesPerPixel)
                {
                    ////double pixelValue = map[row, col];
                    ////byte value = (byte)(byte.MaxValue * (1 - pixelValue));
                    MapCell pixel = map.GetCell(row, col);
                    float value = 0;

                    if (pixel.Weight > 0)
                    {
                        if (pixel.Value < 0)
                        {
                            value = -(NearBehind + (NearBehind - FarBehind) * pixel.Value);
                        }
                        else
                        {
                            value = (NearFront - (NearFront - farFront) * pixel.Value);
                        }

                        if (!this.ShowAllReadings && value != 0)
                        {
                            value = value * Math.Min(1, pixel.Weight);
                            
                            // clear all obstacle noise and retain just the contour
                            ////if (value < 0)
                            ////{
                            ////    if (map.GetCell(row, col + 1).Value > 0 ||
                            ////        map.GetCell(row, col - 1).Value > 0 ||
                            ////        map.GetCell(row + 1, col).Value > 0 ||
                            ////        map.GetCell(row + 1, col + 1).Value > 0 ||
                            ////        map.GetCell(row + 1, col - 1).Value > 0 ||
                            ////        map.GetCell(row - 1, col).Value > 0 ||
                            ////        map.GetCell(row - 1, col + 1).Value > 0 ||
                            ////        map.GetCell(row - 1, col - 1).Value > 0)
                            ////    {
                            ////    }
                            ////    else
                            ////    {
                            ////        value = FarBehind;
                            ////    }
                            ////}
                        }
                    }

                    this.rawImage[index] = (byte)(Neutral + value);
                }
            }
             
            this.GlobalMapVideoFrame = new VideoFrame()
            {
                Data = this.rawImage,
                Format = SimplePixelFormat.Gray8,
                Stride = stride,
                Size = new System.Drawing.Size(width, height)
            };

            this.RobotVisibility = Visibility.Visible;
        }

        /// <summary>
        /// Visualize Origin
        /// </summary>
        private void UpdateOrigin()
        {
            this.Origin = new Pose2D(this.GlobalMapWidth / 2, this.GlobalMapImageHeight / 2);
        }
    }
}
